function [scotu,scotp,U] = scot(qsol,a)

tq = qsol(1,:);
x = qsol(2:end,:);
x0 = x(:,1);
xf = x(:,end);
p0 = phip_sca(x0);
ep = a(1,5);

U = [];
Unorm = [];
Pnorm = [];


for i = 1:length(tq)
    [u,unorm,pnorm] = u_norm(x(:,i),p0,a,ep);
    U = [U, u];
    Unorm = [Unorm; unorm];
    Pnorm = [Pnorm; pnorm];
end


UnormInt = trapz(tq,Unorm);
PnormInt = trapz(tq,Pnorm);
posf = jpos_mat(xf);
steplength = posf(1,end);

weight = 2.40295;

scotu = (1/steplength)*(1/weight)*UnormInt;
scotp = (1/steplength)*(1/weight)*PnormInt;

end

function [u,unorm,pnorm] = u_norm(q, p, a, ep)


ndof = length(q)/2;
dx = q(ndof+1:end);
% q
% pause(1000)
M    = D_mat(q);
C    = C_mat(q);
G    = G_vec(q);

% pause(0.5);

%%%Outputs
y_a1     = ya1_sca(q, p, a);
y_a2     = ya2_vec(q, p, a);
y_d1     = yd1_sca(q, p,  a);
y_d2     = yd2_vec(q, p,  a);

%%%Outputs
y1 = y_a1 - y_d1;
y2 = y_a2 - y_d2;


y_a = [y_a1; y_a2];
y_d = [y_d1; y_d2];

%%%Jacobians of Outputs
Dy_a1    = Dya1_mat(q, p, a);
Dy_d1    = Dyd1_mat(q, p, a);
Dy_a2    = Dya2_mat(q, p, a);
Dy_d2    = Dyd2_mat(q, p, a);

Dy_1 = Dy_a1 - Dy_d1;
Dy_2 = Dy_a2 - Dy_d2;

%%%Control Fields

vf    = [dx; M \ (-C*dx - G)];
B_IO  = eye(ndof);
gf    = [zeros(size(B_IO)); M \ B_IO];

%%%Lie Derivatives
Lgy1 = Dy_1*gf;
Lgy2 = Dy_2*gf;
Lfy1 = Dy_1*vf;
Lfy2 = Dy_2*vf;

%%%Second Order Jacobians

DLfy_a2  = DLfya2_mat(q, p, a);
DLfy_d2  = DLfyd2_mat(q, p, a);

DLfy2 = DLfy_a2-DLfy_d2;

%%%Second Lie Derivatives

LfLfy2 = DLfy2*vf;
LgLfy2 = DLfy2*gf;

% tp = [tp, t+tp0];
% yact = [yact, y_a];
% ydes = [ydes, y_d];

% Vector field

A = [Lgy1; LgLfy2];

u = -A \ ([0; LfLfy2]+[Lfy1; 2*ep*Lfy2]+ [ep*y1; ep^2*y2]);

unorm = sum(abs(u));
%pnorm = sum(abs(dx.*u));
pnorm = abs(transpose(dx)*u);

% U = [U, u];
% Pow = [Pow, u.*dx];

% ret = u;

end
